#pragma once
// ============================================================================
// 文件：include/SLAM/landmark_manager.hpp
// 说明：全局反光柱地标维护（严格判定）
//      - 最近邻距离门控 + 半径一致性门控
//      - 命中计数 hits（达到阈值才“确认”）
//      - 老化/超时剔除
//      - 与衰减模型配合（由外部在成功匹配到“确认地标”时喂样本）
// ============================================================================

#include "SLAM/common.hpp"
#include "SLAM/attenuation_model.hpp"
#include "HungarianAlgorithm.hpp"

namespace SLAM {
/*
struct Track {
  int id{-1};
  double x{0}, y{0};
  Cov2 P{0.2, 0.0, 0.2};          // 初始协方差
  double radius{0.08};
  double strength{0.0};
  int hits{0};
  double last_seen_sec{0.0};
  bool confirmed{false};
};
*/
struct LMParams {
  int    min_hits_confirm{5};     // 至少命中5次才确认地标
  double max_age_sec{3.0};        // 超过该时间未被观测则淘汰
  double merge_radius{0.25};      
  double nn_gate_m{1.2};          // 最近邻关联门限（米）
  bool   use_ekf{true};           // 是否使用ekf
  double q_process{1e-4};         // 过程噪声
  double r_meas{0.05};            // 观测噪声（位置）
};
/*
  struct Params {
    int    min_hits_confirm{3};   // 至少命中3次才确认地标
    double nn_gate{0.5};          // 最近邻关联门限（米）
    double radius_gate_ratio{0.5};// 半径允许的相对偏差（±50%）
    double max_age_sec{8.0};      // 超过该时间未被观测则淘汰
    double max_distance_threshold_{0.5};
  };
*/

class LandmarkManager {
public:

  explicit LandmarkManager(const LMParams& p, rclcpp::Clock::SharedPtr clock)
  : p_(p), clock_(std::move(clock)) {}
  

  // 提供当前时间（秒）给 manager
  void set_now(double tsec){ now_sec_ = tsec; }


  // ================== 地标数据关联/维护 ==================

  // 外部提供的候选匹配（已经过匈牙利与马氏 gating）
  // 仅在 “高可信帧” 时做更新，否则只做命中计数与 last_seen 维护（不改位置）
  void fuse(const std::vector<Reflector>& obs_map,
            const std::vector<MatchPair>& matches,
            bool high_conf_frame)
  {
    // 先把所有 Landmark 打上“未命中”；
    std::unordered_map<int,bool> matched;
    matched.reserve(lms_.size());

    for (auto &mp : matches){
      int tid = mp.lm_index;
      if (!has(tid)) continue;
      matched[tid] = true;

      // 更新 last_seen
      auto &T = lms_[tid];
      T.last_seen_sec = now_sec_;
      T.hits = std::min(T.hits+1, 100000);

      if (!high_conf_frame) { L.hits++; continue; } // 低可信帧只更新时间/命中

      const auto &z = obs_map[mp.obs_index];
      if (p_.use_ekf) ekfUpdate(T, z);
      else emaUpdate(T, z);
    }

    // 年久失修的删掉
    garbageCollect();

    // 合并近邻重复点（周期性）
    if (now_sec_ - last_merge_ > 1.0) {
      mergeCloseTracks();
      last_merge_ = now_sec_;
    }
  }
  
  // 新增或补充：当某些观测没有匹配到任何 track，并且是高可信帧，可创建新 Landmark
  void spawn_unmatched(const std::vector<Reflector>& obs_map,
                       const std::vector<bool>& is_matched,
                       bool high_conf_frame)
  {
    if (!high_conf_frame) return;
    for (size_t i=0;i<obs_map.size();++i){
      if (is_matched[i]) continue;
      Landmark T;
      T.id = next_id_++;
      T.x = obs_map[i].x; T.y = obs_map[i].y;
      T.P = {0.5,0.0,0.5};
      T.radius = obs_map[i].radius;
      T.strength = obs_map[i].strength;
      T.hits = 1;
      T.last_seen_sec = now_sec_;
      T.confirmed = (T.hits >= p_.min_hits_confirm);
      lms_[T.id] = T;
    }
  }
/*
std::vector<MatchPair> update(const std::vector<Reflector>& obs_map,
                              const rclcpp::Time& stamp)
{
  std::lock_guard<std::mutex> lk(mu_);
  std::vector<MatchPair> matches;

  const int M = obs_map.size();
  const int N = lms_.size();
  if (M == 0 && N == 0) return matches;

  // ---------- 快速路径：无地标 → 全部新建（hits=1） ----------
  if (N == 0) {
    for (const auto& o : obs_map) {
      Landmark L;
      L.id        = next_id_++;
      L.x         = o.x;  L.y = o.y;
      L.radius    = o.radius;
      L.strength  = o.strength;
      L.hits      = 1;                 // ★ 新地标从 1 起
      L.last_seen = stamp;
      lms_[L.id]  = L;
    }
    return matches;
  }

  // ---------- 分组：已确认 / 暂态 ----------
  std::vector<int> lm_ids_confirmed, lm_ids_tentative;
  std::vector<Landmark*> lm_ptrs_confirmed, lm_ptrs_tentative;
  for (auto& kv : lms_) {
    if (kv.second.hits >= p_.min_hits_confirm) {
      lm_ids_confirmed.push_back(kv.first);
      lm_ptrs_confirmed.push_back(&kv.second);
    } else {
      lm_ids_tentative.push_back(kv.first);
      lm_ptrs_tentative.push_back(&kv.second);
    }
  }

  // ---------- 构造代价矩阵（行=观测，列=地标） ----------
  auto build_cost = [&](const std::vector<Reflector>& obs,
                        const std::vector<Landmark*>& lm_ptrs)
  {
    const double INF = 1e6;
    const double w_d = 1.0;
    const double w_r = 1.0;

    std::vector<std::vector<double>> cost;
    cost.resize(obs.size(), std::vector<double>(lm_ptrs.size(), INF));

    for (int i = 0; i < obs.size(); ++i) {
      const auto& o = obs[i];
      for (int j = 0; j < lm_ptrs.size(); ++j) {
        const auto& L = *lm_ptrs[j];
        double d = std::hypot(o.x - L.x, o.y - L.y);
        if (d > p_.nn_gate) continue;

          if (L.radius > 0 && o.radius > 0) {
            const double rmin = L.radius * (1.0 - p_.radius_gate_ratio);
            const double rmax = L.radius * (1.0 + p_.radius_gate_ratio);
          if (o.radius < rmin || o.radius > rmax) continue;
          }

        const double rdiff = (L.radius > 0 && o.radius > 0) ? std::abs(o.radius - L.radius) : 0.0;
        cost[i][j] = w_d * d + w_r * rdiff;
          }
    }
    return cost;
  };

  // ---------- 工具：应用指派，更新被匹配地标（EMA + hits++），填充 matches ----------
  auto apply_assignment = [&](const std::vector<Reflector>& obs,
                              const std::vector<int>& lm_ids,
                              const std::vector<Landmark*>& lm_ptrs,
                              const std::vector<int>& assignment,
                              std::unordered_set<int>& matched_lm_ids,
                              std::vector<int>& used_obs)
  {
    const double alpha = 0.2;
    for (int i = 0; i < obs.size(); ++i) {
      int j = assignment[i];
      if (j < 0 || j >= lm_ptrs.size()) continue;

      Landmark& L = *lm_ptrs[j];
      const auto& o = obs[i];

      L.x = (1.0 - alpha) * L.x + alpha * o.x;
      L.y = (1.0 - alpha) * L.y + alpha * o.y;
      if (o.radius > 0) L.radius = (L.radius <= 0) ? o.radius : 0.9 * L.radius + 0.1 * o.radius;
      if (o.strength > 0)
        L.strength = (L.strength <= 0) ? o.strength : (1.0 - alpha) * L.strength + alpha * o.strength;

      L.hits = std::min(L.hits + 1, kHitsCap);
      L.last_seen = stamp;
      matches.push_back(MatchPair{lm_ids[j], i});
      matched_lm_ids.insert(lm_ids[j]);
      used_obs[i] = 1;
    }
  };

  // ---------- 阶段 1：已确认地标 ----------
  std::unordered_set<int> matched_lm_ids;
  std::vector<int> used_obs(M, 0);

  if (!lm_ptrs_confirmed.empty()) {
    auto cost1 = build_cost(obs_map, lm_ptrs_confirmed);
    std::vector<int> assignment1(M, -1);
      HungarianAlgorithm hungarian1;
      hungarian1.Solve(cost1, assignment1, p_.max_distance_threshold_);
    apply_assignment(obs_map, lm_ids_confirmed, lm_ptrs_confirmed, assignment1, matched_lm_ids, used_obs);
  }

  // ---------- 阶段 2：剩余观测匹配暂态地标 ----------
  if (!lm_ptrs_tentative.empty()) {
    std::vector<Reflector> obs_remain;
    std::vector<int> obs_remain_idx_map;
    for (int i = 0; i < M; ++i) {
      if (used_obs[i] == 0) {
        obs_remain.push_back(obs_map[i]);
        obs_remain_idx_map.push_back(i);
      }
    }

    if (!obs_remain.empty()) {
      auto cost2 = build_cost(obs_remain, lm_ptrs_tentative);
      std::vector<int> assignment2(obs_remain.size(), -1);
      HungarianAlgorithm hungarian2;
      hungarian2.Solve(cost2, assignment2, p_.max_distance_threshold_);

      // 重新映射回全局索引
      std::vector<int> full_assignment(M, -1);
      for (int k = 0; k < assignment2.size(); ++k)
        if (assignment2[k] >= 0)
          full_assignment[obs_remain_idx_map[k]] = assignment2[k];

      apply_assignment(obs_map, lm_ids_tentative, lm_ptrs_tentative, full_assignment,
                       matched_lm_ids, used_obs);
    }
  }

  // ---------- 新建未匹配观测 ----------
  for (int i = 0; i < M; ++i) {
      if (used_obs[i] == 0) {
      const auto& o = obs_map[i];
      Landmark L;
      L.id        = next_id_++;
      L.x         = o.x;
      L.y         = o.y;
      L.radius    = o.radius;
      L.strength  = o.strength;
      L.hits      = 1;
      L.last_seen = stamp;
      lms_[L.id]  = L;
    }
  }

  // ---------- 仅对“未匹配的地标”衰减，并做超时删除 ----------
  {
    const auto now = clock_->now();
    std::vector<int> to_del;
    for (auto& kv : lms_) {
      const int id = kv.first;
      auto&      L = kv.second;

      if (matched_lm_ids.find(id) == matched_lm_ids.end()) {
        L.hits = std::max(0, L.hits - 1);  // ★ 未匹配 → 衰减
      }

      const double dt = (now - L.last_seen).seconds();
      if (dt > p_.max_age_sec && L.hits < p_.min_hits_confirm) {
        to_del.push_back(id);
      }
    }
    for (int id : to_del) lms_.erase(id);
  }

  return matches;
}
*/
  // 导出协方差（与 all() 顺序对应 by id->index 的外部映射负责）
  std::vector<Cov2> all_cov_by_order(const std::vector<Landmark>& by_order) const {
    std::vector<Cov2> out; out.reserve(by_order.size());
    for (auto &L : by_order) {
      auto it = lms_.find(L.id);
      if (it==lms_.end()) out.push_back({0.2,0,0.2});
      else out.push_back(it->second.P);
    }
    return out;
  }

  bool has(int id) const { return lms_.count(id)>0; }
  void ekfUpdate(Landmark& T, const Reflector& z) {
    Eigen::Vector2d mu(T.x, T.y);
    Eigen::Matrix2d P  = T.P;
    Eigen::Vector2d zi(z.x, z.y);
    Eigen::Matrix2d R  = R_from_range_intensity(z);

    Eigen::Vector2d v = zi - mu;
    Eigen::Matrix2d S = P + R;
    Eigen::Matrix2d K = P * S.inverse();

    Eigen::Vector2d mu_new = mu + K * v;
    Eigen::Matrix2d P_new  = (Eigen::Matrix2d::Identity() - K) * P;

    T.x = mu_new.x(); T.y = mu_new.y(); T.P = P_new;
    T.strength = z.strength;
    T.radius   = 0.5*T.radius + 0.5*z.radius;
    T.hits++; T.last_seen = now_;    

    if (!T.confirmed && T.hits >= p_.min_hits_confirm) T.confirmed = true;

    /*
    // 预测：x'=x, P'=P + Q
    T.P.xx += p_.q_process; T.P.yy += p_.q_process;

    // 观测 H=[1 0; 0 1], R=r*I
    const double rx = p_.r_meas, ry = p_.r_meas;
    const double Sxx = T.P.xx + rx*rx;
    const double Sxy = T.P.xy;
    const double Syy = T.P.yy + ry*ry;
    const double det = Sxx*Syy - Sxy*Sxy;
    if (det <= 1e-12) return;

    // K = P H^T S^-1 = P S^-1
    const double inv_xx =  Syy/det;
    const double inv_xy = -Sxy/det;
    const double inv_yy =  Sxx/det;

    const double Kxx = T.P.xx*inv_xx + T.P.xy*inv_xy;
    const double Kxy = T.P.xx*inv_xy + T.P.xy*inv_yy;
    const double Kyx = T.P.xy*inv_xx + T.P.yy*inv_xy;
    const double Kyy = T.P.xy*inv_xy + T.P.yy*inv_yy;

    const double dx = z.x - T.x;
    const double dy = z.y - T.y;

    T.x += Kxx*dx + Kxy*dy;
    T.y += Kyx*dx + Kyy*dy;

    // P = (I-K)P
    const double Pxx = (1.0-Kxx)*T.P.xx - Kxy*T.P.xy;
    const double Pxy = (1.0-Kxx)*T.P.xy - Kxy*T.P.yy;
    const double Pyx = -Kyx*T.P.xx + (1.0-Kyy)*T.P.xy;
    const double Pyy = -Kyx*T.P.xy + (1.0-Kyy)*T.P.yy;
    T.P.xx = std::max(1e-6, Pxx);
    T.P.xy = 0.5*(Pxy+Pyx);
    T.P.yy = std::max(1e-6, Pyy);

    T.strength = z.strength;
    T.radius   = 0.5*T.radius + 0.5*z.radius;

    if (!T.confirmed && T.hits >= p_.min_hits_confirm) T.confirmed = true;
    */
  }

  void emaUpdate(Landmark& T, const Reflector& z) {
    const double alpha = 0.2;
    T.x = (1-alpha)*T.x + alpha*z.x;
    T.y = (1-alpha)*T.y + alpha*z.y;
    T.P.xx = std::max(1e-4, (1-alpha)*T.P.xx + alpha*p_.r_meas*p_.r_meas);
    T.P.yy = std::max(1e-4, (1-alpha)*T.P.yy + alpha*p_.r_meas*p_.r_meas);
    T.strength = z.strength;
    T.radius   = 0.5*T.radius + 0.5*z.radius;
    if (!T.confirmed && T.hits >= p_.min_hits_confirm) T.confirmed = true;
  }

  void garbageCollect() {
    std::vector<int> to_del;
    to_del.reserve(lms_.size());
    for (auto &kv : lms_){
      if (now_sec_ - kv.second.last_seen_sec > p_.max_age_sec) to_del.push_back(kv.first);
    }
    for (int id : to_del) lms_.erase(id);
  }

  void mergeCloseTracks() {
    // 简易半径合并：近邻合并成加权中心
    std::vector<int> ids; ids.reserve(lms_.size());
    for (auto &kv : lms_) ids.push_back(kv.first);
    for (size_t i=0;i<ids.size();++i){
      if (!lms_.count(ids[i])) continue;
      for (size_t j=i+1;j<ids.size();++j){
        if (!lms_.count(ids[j])) continue;
        auto &A = lms_[ids[i]];
        auto &B = lms_[ids[j]];
        const double d = std::hypot(A.x-B.x, A.y-B.y);
        if (d <= p_.merge_radius){
          const double wa = std::min(1.0, 1.0/(A.P.xx+A.P.yy+1e-6));
          const double wb = std::min(1.0, 1.0/(B.P.xx+B.P.yy+1e-6));
          const double w = wa+wb+1e-9;
          A.x = (wa*A.x + wb*B.x)/w;
          A.y = (wa*A.y + wb*B.y)/w;
          A.P.xx = std::min(A.P.xx, B.P.xx);
          A.P.yy = std::min(A.P.yy, B.P.yy);
          A.radius = 0.5*(A.radius + B.radius);
          A.strength = std::max(A.strength, B.strength);
          A.hits += B.hits;
          A.confirmed = A.confirmed || B.confirmed;
          lms_.erase(B.id);
        }
      }
    }
  }  
  // 导出“确认地标”（供后端定位与可视化）
  std::vector<Landmark> confirmed() const {
    std::vector<Landmark> out;
    out.reserve(lms_.size());
    for (auto &kv : lms_){
      const auto &T = kv.second;
      if (!T.confirmed) continue;
      out.push_back({ T.id, T.x, T.y, T.radius, T.strength, T.hits });
    }
    return out;
  }

  // 导出全部（含未确认，调试用）
  std::vector<Landmark> all() const {
    std::vector<Landmark> out;
    out.reserve(lms_.size());
    for (auto &kv : lms_){
      const auto &T = kv.second;
      out.push_back({ T.id, T.x, T.y, T.radius, T.strength, T.hits });
    }
    return out;
  }
  AttenuationModel& attenuation() const { return atten_; }
private:
  LMParams p_;
  std::unordered_map<int, Landmark> lms_;
  int next_id_{1};
  double now_sec_{0.0};
  double last_merge_{-1e9};
  AttenuationModel atten_;
};

} // namespace SLAM
